#include "bsp/bsp.h"
#include "gpio.h"

#define N_EXTI 16

struct subscription_t {
    u32  port;
    void (*callback)(void);
    void* ctx;
} subscriptions[N_EXTI];

static u8 clk_enabled_[PORT_MAX] = {0};

static __INLINE u16 get_pin_number(u16 pin_mask_) {
	uint16_t pin_number = 0;
	uint16_t pin_mask = pin_mask_ >> 1;
	while (pin_mask) {
		pin_mask >>= 1;
		pin_number++;
	}
	return pin_number;
}

static inline IRQn_Type get_irq_number(u16 pin_number) {
    switch (pin_number) {
        case 0: return EXINT0_IRQn;
        case 1: return EXINT1_IRQn;
        case 2: return EXINT2_IRQn;
        case 3: return EXINT3_IRQn;
        case 4: return EXINT4_IRQn;
        case 5:
        case 6:
        case 7:
        case 8:
        case 9: return EXINT9_5_IRQn;
        case 10:
        case 11:
        case 12:
        case 13:
        case 14:
        case 15: return EXINT15_10_IRQn;
        default: return (IRQn_Type)0; // impossible
    }
}


static inline gpio_type *get_gpio_port(u32 io) {
	u16 io_port = PORT(io);
    switch (io_port) {
        case 0: return GPIOA;
        case 1: return GPIOB;
        case 2: return GPIOC;
        case 3: return GPIOD;
        //case 4: return GPIOE;
        case 5: return GPIOF;
        //case 6: return GPIOG;
        default: return (gpio_type *)NULL; // impossible
    }
}

static void _gpio_enable_clk(u32 io) {
  	gpio_type *port = get_gpio_port(io);
	if (port == NULL) {
		return;
	}
    if (port == GPIOA) {
        crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE);
		clk_enabled_[0] = 1;
    } else if (port == GPIOB) {
        crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE);
		clk_enabled_[1] = 1;
    } else if (port == GPIOC) {
        crm_periph_clock_enable(CRM_GPIOC_PERIPH_CLOCK, TRUE);
		clk_enabled_[2] = 1;
    } else if (port == GPIOD) {
        crm_periph_clock_enable(CRM_GPIOD_PERIPH_CLOCK, TRUE);
		clk_enabled_[3] = 1;
    } else if (port == GPIOF) {
        crm_periph_clock_enable(CRM_GPIOF_PERIPH_CLOCK, TRUE);
		clk_enabled_[5] = 1;
    }else {
        return;
    }
}

void gpio2_init(u32 io, gpio_mode_type mode, gpio_drive_type drv_str, gpio_output_type outtype, gpio_pull_type pull) {
  	gpio_type *port = get_gpio_port(io);
	if (port == NULL) {
		return;
	}
	_gpio_enable_clk(io);
	gpio_init_type init_var;
	init_var.gpio_mode = mode;
	init_var.gpio_pins = PIN(io);
	init_var.gpio_drive_strength = drv_str;
	init_var.gpio_out_type = outtype;
	init_var.gpio_pull = pull;
	gpio_init(port, &init_var);
}


void gpio2_init_output(u32 io, gpio_output_type outtype, gpio_pull_type pull){
	gpio2_init(io, GPIO_MODE_OUTPUT, GPIO_DRIVE_STRENGTH_STRONGER, outtype, pull);
}

void gpio2_init_input(u32 io, gpio_pull_type pull) {
	gpio2_init(io, GPIO_MODE_INPUT, GPIO_DRIVE_STRENGTH_STRONGER, GPIO_OUTPUT_PUSH_PULL, pull);
}

void gpio2_init_can(u32 rx_pio, u32 tx_pio)
{
	gpio2_init_input(rx_pio, GPIO_PULL_UP);
	gpio2_init(tx_pio, GPIO_MODE_MUX, GPIO_DRIVE_STRENGTH_STRONGER, GPIO_OUTPUT_PUSH_PULL, GPIO_PULL_NONE);
}

void gpio2_init_analog(u32 pio) {
	gpio2_init(pio, GPIO_MODE_ANALOG, GPIO_DRIVE_STRENGTH_STRONGER, GPIO_OUTPUT_PUSH_PULL, GPIO_PULL_NONE);
}

void gpio2_write(u32 io, bool set) {
  	gpio_type *port = get_gpio_port(io);
	if (port == NULL) {
		return;
	}
	gpio_bits_write(port, PIN(io), set ? TRUE : FALSE);
}

bool gpio2_read(u32 io) {
  	gpio_type *port = get_gpio_port(io);
	if (port == NULL) {
		return false;
	}
	return (PIN(io) & port->idt) != 0;
}

void gpio2_output_write(u32 io, bool set) {
	if (clk_enabled_[PORT(io)] == 0) {
		_gpio_enable_clk(io);
	}
    gpio2_write(io, set);
    gpio2_init_output(io, GPIO_OUTPUT_PUSH_PULL, GPIO_PULL_NONE);
}


#define io_2_exitline(io) (PIN(io))

void exti_subscribe(u32 io, u32 edge, exti_handler handler) {
	exint_polarity_config_type trig;
	if (edge == EXINT_TRIG_ALL) {
		trig = EXINT_TRIGGER_BOTH_EDGE;
	}else if (edge == EXINT_TRIG_RAISING) {
		trig = EXINT_TRIGGER_RISING_EDGE;
	}else {
		trig = EXINT_TRIGGER_FALLING_EDGE;
	}

	gpio2_init_input(io, GPIO_PULL_NONE);

	u16 pin_number = get_pin_number(PIN(io));
	IRQn_Type n_irq = get_irq_number(pin_number);
	if (n_irq == 0 || pin_number >= N_EXTI) {
		return;
	}
	__disable_irq();
	subscriptions[pin_number].callback = handler;
	subscriptions[pin_number].ctx = NULL;
	subscriptions[pin_number].port = NULL;
	__enable_irq();
	exint_init_type exint_init_struct;

	exint_init_struct.line_enable = TRUE;
	exint_init_struct.line_mode = EXINT_LINE_INTERRUPUT;
	exint_init_struct.line_select = io_2_exitline(io);
	exint_init_struct.line_polarity = trig;
	exint_init(&exint_init_struct);
	exint_flag_clear(io_2_exitline(io));
	nvic_irq_enable(n_irq, LOW_IRQ_PRIORITY, 0U);
}
void exti_unsubscribe(u32 io) {
	exint_flag_clear(io_2_exitline(io));
	exint_interrupt_enable(io_2_exitline(io), FALSE);
	uint16_t pin_number = get_pin_number(PIN(io));
	if (pin_number >= N_EXTI) {
		return;
	}
	subscriptions[pin_number].callback = NULL;
	subscriptions[pin_number].ctx = NULL;
	subscriptions[pin_number].port = NULL;
}

static __INLINE void exti_handle(u16 exti_number) {
    if(exint_flag_get(1 << exti_number) == RESET) {
        return; // This interrupt source did not trigger the interrupt line
    }

    exint_flag_clear(1 << exti_number);

    struct subscription_t *subscription = &subscriptions[exti_number];
    if (subscription->callback) {
        (subscription->callback)();
    }
}

bool exti_is_pending_clear(u32 io) {
	if (exint_flag_get(io_2_exitline(io)) == SET){
		exint_flag_clear(io_2_exitline(io));
		return true;
	}
	return false;
}

/** @brief Entrypoint for the EXTI line 0 interrupt. */
void EXINT0_IRQHandler(void) {
    exti_handle(0);
}

/** @brief Entrypoint for the EXTI line 1 interrupt. */
void EXINT1IRQHandler(void) {
    exti_handle(1);
}

/** @brief Entrypoint for the EXTI line 2 interrupt. */
void EXINT2_IRQHandler(void) {
    exti_handle(2);
}

/** @brief Entrypoint for the EXTI line 3 interrupt. */
void EXINT3_IRQHandler(void) {
    exti_handle(3);
}

/** @brief Entrypoint for the EXTI line 4 interrupt. */
void EXINT4_IRQHandler(void) {
    exti_handle(4);
}

/** @brief Entrypoint for the EXTI lines 5-9 interrupt. */
void EXINT9_5_IRQHandler(void) {
    exti_handle(5);
    exti_handle(6);
    exti_handle(7);
    exti_handle(8);
    exti_handle(9);
}

/** @brief This function handles EXTI lines 10-15 interrupt. */
void EXINT15_10_IRQHandler(void) {
    exti_handle(10);
    exti_handle(11);
    exti_handle(12);
    exti_handle(13);
    exti_handle(14);
    exti_handle(15);
}

